#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include<tf/transform_datatypes.h>
#include<Eigen/Eigen>
#include<fstream>

bool is_get_tracker_first_gps_odom_ = false;
tf::StampedTransform tf_tracker_first_,tf_tracker_last_;
tf::Vector3 tracker_position_last_;

ros::Publisher tracker_odom_pub_,tracker_odom_path_pub_, tracker_velocity_pub_;
nav_msgs::Path tracker_odom_path_;

bool is_get_target_first_gps_odom_ = false;
tf::StampedTransform tf_target_last_,tf_target_first_;
Eigen::Vector3d target_position_last_(0,0,0);

ros::Publisher target_odom_pub_,target_odom_path_pub_;
nav_msgs::Path target_odom_path_;

double tracker_odom_first_yaw_;

//raed result
std::ifstream det_result_file_;

ros::Publisher det_target_odom_pub_,det_target_odom_path_pub_;
nav_msgs::Path det_target_odom_path_;

int pub_file_count = 0;
double angle_adjust_ = 0,x_adjust_=0,y_adjust_=0,time_adjust_=0;

void saveTrajectory(std::string& out_file,double time_stamp, double x,double y,double z,double ow,double ox,double oy,double oz, double vx,double vy,double vz)
{
    std::ofstream outfile(out_file.c_str(),std::ios::out|std::ios::binary|std::ios::app);
    if(!outfile) std::cout<<"error"<<std::endl;
    outfile<<std::to_string(time_stamp)<<" "<<std::to_string(x)<<" "<<std::to_string(y)<<" "<<std::to_string(z)<<" "    //xyz
           <<std::to_string(ow)<<" "<<std::to_string(ox)<<" "<<std::to_string(oy)<<" "<<std::to_string(oz)<<" "<<
           std::to_string(vx)<<" "<<std::to_string(vy)<<" "<<std::to_string(vz)<<std::endl;  //
    outfile.close();
}

void pub_det_result(){
    pub_file_count++;
    if(pub_file_count%5)
        return;
    std::string s_line, time_stamp,object_id,
        cur_abs_pos1,cur_abs_pos2,cur_abs_pos3,
        cur_abs_vel1,cur_abs_vel2,cur_abs_vel3,
        pre_abs_pos11,pre_abs_pos12,pre_abs_pos13,
        pre_abs_pos21,pre_abs_pos22,pre_abs_pos23,
        pre_abs_pos31,pre_abs_pos32,pre_abs_pos33,
        pre_abs_pos41,pre_abs_pos42,pre_abs_pos43,
        pre_abs_pos51,pre_abs_pos52,pre_abs_pos53,
        pre_abs_pos61,pre_abs_pos62,pre_abs_pos63,
        pre_abs_pos71,pre_abs_pos72,pre_abs_pos73,
        pre_abs_pos81,pre_abs_pos82,pre_abs_pos83,
        pre_abs_pos91,pre_abs_pos92,pre_abs_pos93,
        pre_abs_pos101,pre_abs_pos102,pre_abs_pos103;
    if(std::getline(det_result_file_,s_line)){
        std::istringstream s_in(s_line);
        s_in>>time_stamp>>object_id>>
            cur_abs_pos1>>cur_abs_pos2>>cur_abs_pos3>>
            cur_abs_vel1>>cur_abs_vel2>>cur_abs_vel3>>
            pre_abs_pos11>>pre_abs_pos12>>pre_abs_pos13>>
            pre_abs_pos21>>pre_abs_pos22>>pre_abs_pos23>>
            pre_abs_pos31>>pre_abs_pos32>>pre_abs_pos33>>
            pre_abs_pos41>>pre_abs_pos42>>pre_abs_pos43>>
            pre_abs_pos51>>pre_abs_pos52>>pre_abs_pos53>>
            pre_abs_pos61>>pre_abs_pos62>>pre_abs_pos63>>
            pre_abs_pos71>>pre_abs_pos72>>pre_abs_pos73>>
            pre_abs_pos81>>pre_abs_pos82>>pre_abs_pos83>>
            pre_abs_pos91>>pre_abs_pos92>>pre_abs_pos93>>
            pre_abs_pos101>>pre_abs_pos102>>pre_abs_pos103;

        if (object_id == "0")
            return;

        //pub odom
        nav_msgs::Odometry target_odom_cur_msg;
        target_odom_cur_msg.header.frame_id = "world";
        target_odom_cur_msg.child_frame_id = "det_target";
        target_odom_cur_msg.header.stamp = ros::Time::now();
        geometry_msgs::Pose pose_msg_temp;
        pose_msg_temp.position.x = atof(cur_abs_pos1.c_str());
        pose_msg_temp.position.y = atof(cur_abs_pos2.c_str());
        pose_msg_temp.position.z = atof(cur_abs_pos3.c_str());
        // ROS_DEBUG_STREAM("det pos: "<<pose_msg_temp.position.x<<" "<<pose_msg_temp.position.y<<" "<<pose_msg_temp.position.z);
        pose_msg_temp.orientation.w= 1;
        pose_msg_temp.orientation.x = 0;
        pose_msg_temp.orientation.y = 0;
        pose_msg_temp.orientation.z = 0;

        target_odom_cur_msg.pose.pose = pose_msg_temp;
        target_odom_cur_msg.twist.twist.linear.x = 0;
        target_odom_cur_msg.twist.twist.linear.y = 0;
        target_odom_cur_msg.twist.twist.linear.z = 0;
        det_target_odom_pub_.publish(target_odom_cur_msg);

        //pub path
        geometry_msgs::PoseStamped pose_stamped_temp;           
        pose_stamped_temp.pose=pose_msg_temp;
        pose_stamped_temp.header.stamp=ros::Time::now();
        pose_stamped_temp.header.frame_id="world";
        det_target_odom_path_.header=pose_stamped_temp.header;
        det_target_odom_path_.poses.emplace_back(pose_stamped_temp);
        det_target_odom_path_pub_.publish(det_target_odom_path_);
    }
}

void tracker_gps_odom_CB(const nav_msgs::OdometryConstPtr& tracker_gps_odom){
    if(!is_get_tracker_first_gps_odom_){
        tf::Transform tf_temp;
        tf::poseMsgToTF(tracker_gps_odom->pose.pose, tf_temp);
        tf_tracker_first_ = tf::StampedTransform(tf_temp, tracker_gps_odom->header.stamp, "gps", "tracker");
        tf_tracker_last_ = tf_tracker_first_;
        double roll_temp,pitch_temp;
        tf::Matrix3x3(tf_tracker_first_.getRotation()).getRPY(roll_temp,pitch_temp, tracker_odom_first_yaw_);
        tracker_odom_first_yaw_ = -tracker_odom_first_yaw_;
        is_get_tracker_first_gps_odom_ = true;
        ROS_INFO_STREAM("get first tracker gps odom: "<<tf_tracker_first_.stamp_.toSec()<<" "<<
            tf_tracker_first_.getOrigin().getX()<<" "<<tf_tracker_first_.getOrigin().getY()<<" "<<tf_tracker_first_.getOrigin().getZ());
        return;
    }
    tf::Transform tf_temp;
    tf::poseMsgToTF(tracker_gps_odom->pose.pose, tf_temp);
    tf::StampedTransform tf_tracker_cur(tf_temp, tracker_gps_odom->header.stamp, "gps", "tracker");

    tf::Vector3 tracker_position(tf_tracker_cur.getOrigin().getX() - tf_tracker_first_.getOrigin().getX(), 
                        tf_tracker_cur.getOrigin().getY()-tf_tracker_first_.getOrigin().getY(), 0);
    tf::Transform tf_convert;
    tf_convert.setOrigin(tf::Vector3(0,0,0));
    tf_convert.setRotation(tf::Quaternion(0,0,tracker_odom_first_yaw_));
    tracker_position = tf_convert*tracker_position;
    tracker_position = tf::Vector3(tracker_position[0], -tracker_position[1], 0);
    tf_convert.setRotation(tf::Quaternion(0,0,angle_adjust_));
    tracker_position = tf_convert*tracker_position;
    tracker_position = tf::Vector3(tracker_position[0]+x_adjust_, tracker_position[1]+y_adjust_, 0);

    tf::Transform tf_delta = tf_tracker_first_.inverse()*tf_tracker_cur;
    double roll_temp,pitch_temp,yaw_temp;
    tf::Matrix3x3(tf_delta.getRotation()).getRPY(roll_temp,pitch_temp,yaw_temp);
    tf_delta.setRotation(tf::Quaternion(0, 0, -yaw_temp));
    // ROS_DEBUG_STREAM("tracker yaw: "<<-yaw_temp);
    tf_delta.setOrigin(tracker_position);

    tf::Vector3 tracker_position_cur(tf_delta.getOrigin().getX(),tf_delta.getOrigin().getY(),tf_delta.getOrigin().getZ());
    tf::Vector3 velocity_world = (tracker_position_cur-tracker_position_last_)/(tf_tracker_cur.stamp_-tf_tracker_last_.stamp_).toSec();
    if(abs(velocity_world[0])>10)
        velocity_world[0] = 0;
    if(abs(velocity_world[1])>10)
        velocity_world[1] = 0;
    if(abs(velocity_world[2])>10)
        velocity_world[2] = 0;
    // ROS_DEBUG_STREAM("tracker velocity: "<<std::to_string(velocity_world[0])<<" "<<std::to_string(velocity_world[1])<<" "<<velocity_world[2]);
    // ROS_DEBUG_STREAM("tracker raw: "<<std::to_string(tracker_gps_odom->pose.pose.position.x)<<" "<<velocity_world[1]<<" "<<velocity_world[2]);
    tf_tracker_last_ = tf_tracker_cur;
    tracker_position_last_ = tracker_position_cur;

    //broadcast tf
    static tf::TransformBroadcaster broadcaster;//1.定义一个广播broadcaster
    broadcaster.sendTransform(tf::StampedTransform(tf_delta, ros::Time::now(), "world", "tracker"));

    //pub odom
    nav_msgs::Odometry tracker_odom_cur_msg;
    tracker_odom_cur_msg.header.frame_id = "world";
    tracker_odom_cur_msg.child_frame_id = "tracker";
    tracker_odom_cur_msg.header.stamp = tracker_gps_odom->header.stamp;
    geometry_msgs::Pose pose_msg_temp;
    tf::poseTFToMsg(tf_delta, pose_msg_temp);
    tracker_odom_cur_msg.pose.pose = pose_msg_temp;
    tracker_odom_cur_msg.twist.twist.linear.x = velocity_world[0];
    tracker_odom_cur_msg.twist.twist.linear.y = velocity_world[1];
    tracker_odom_cur_msg.twist.twist.linear.z = velocity_world[2];
    tracker_odom_pub_.publish(tracker_odom_cur_msg);

    std::string pc_path_name_="/home/st/ubuntu_data/exp_record/beijing/0218/brown_gps_gt2.txt";
    saveTrajectory(pc_path_name_,tracker_odom_cur_msg.header.stamp.toSec()+time_adjust_,
                   tracker_odom_cur_msg.pose.pose.position.x,tracker_odom_cur_msg.pose.pose.position.y,tracker_odom_cur_msg.pose.pose.position.z,
                   tracker_odom_cur_msg.pose.pose.orientation.w,tracker_odom_cur_msg.pose.pose.orientation.x,
                   tracker_odom_cur_msg.pose.pose.orientation.y,tracker_odom_cur_msg.pose.pose.orientation.z,
                   tracker_odom_cur_msg.twist.twist.linear.x,tracker_odom_cur_msg.twist.twist.linear.y,tracker_odom_cur_msg.twist.twist.linear.z);

    //pub path
    geometry_msgs::PoseStamped pose_stamped_temp;           
    pose_stamped_temp.pose=pose_msg_temp;
    pose_stamped_temp.header.stamp= tracker_gps_odom->header.stamp;
    pose_stamped_temp.header.frame_id="world";
    tracker_odom_path_.header=pose_stamped_temp.header;
    tracker_odom_path_.poses.emplace_back(pose_stamped_temp);
    tracker_odom_path_pub_.publish(tracker_odom_path_);

    pub_det_result();
}


int main(int argc, char **argv){
    if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug))
    {
        ros::console::notifyLoggerLevelsChanged();
    }

    ros::init(argc, argv, "handle_gps");
    ros::NodeHandle nh;

    nh.param<double>("angle_adjust",angle_adjust_,0.0);
    nh.param<double>("x_adjust",x_adjust_,0.0);
    nh.param<double>("y_adjust",y_adjust_,0.0);
    nh.param<double>("y_adjust",y_adjust_,0.0);
    nh.param<double>("time_adjust",time_adjust_,0.0);

    ros::Subscriber tracker_gps_odom_sub = nh.subscribe("/novatel/oem7/odom", 10, tracker_gps_odom_CB); 
    // ros::Subscriber target_gps_odom_sub = nh.subscribe("/target/novatel/oem7/odom", 10, target_gps_odom_CB); 

    tracker_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/tracker/odom", 1);
    tracker_odom_path_pub_ = nh.advertise<nav_msgs::Path>("/tracker/path", 10,true);
    tracker_velocity_pub_ = nh.advertise<nav_msgs::Path>("/tracker/odom/velocity", 10, true);

    target_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/target/odom", 1);
    target_odom_path_pub_ = nh.advertise<nav_msgs::Path>("/target/path", 10,true);

    det_target_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/det/target/odom", 1);
    det_target_odom_path_pub_ = nh.advertise<nav_msgs::Path>("/det/target/path", 10,true);

    det_result_file_.open("/home/st/ubuntu_data/exp_record/beijing/0218/22.txt");

    tf::Transform test;
    test.setRotation(tf::Quaternion(0,0,-M_PI/2));
    tf::Matrix3x3 T_test(test.getRotation());
    ROS_DEBUG_STREAM("tf "<<T_test[0][0]<<" "<<T_test[0][1]<<" "<<T_test[0][2]);
    ROS_DEBUG_STREAM("tf "<<T_test[1][0]<<" "<<T_test[1][1]<<" "<<T_test[1][2]);
    ROS_DEBUG_STREAM("tf "<<T_test[2][0]<<" "<<T_test[2][1]<<" "<<T_test[2][2]);

    ros::spin();

    return 0;
}
